"""
GomerX 机器人自动化任务主程序
整合所有功能模块，实现完整的自动化任务流程
"""

# 导入标准库
import time      # 时间模块，用于延时和时间控制
import sys       # 系统模块，用于访问系统相关功能
import os        # 操作系统模块，用于文件路径操作
import argparse  # 命令行参数解析模块，用于处理命令行输入

# 添加项目根目录到系统路径
# 这样可以确保无论从哪个目录运行程序，都能正确导入项目模块
sys.path.append(os.path.dirname(os.path.abspath(__file__)))

# 导入自定义模块
from modules.camera import Camera           # 摄像头控制模块
from modules.chassis import Chassis         # 底盘控制模块
from modules.arm import Arm                 # 机械臂控制模块
from modules.gripper import Gripper         # 夹爪控制模块
from modules.navigation import Navigator    # 导航模块
from modules.object_detector import ObjectDetector  # 物品识别模块
from modules.vision import VisionProcessor  # 视觉处理模块
from modules.led import LEDController       # LED控制模块
from config.settings import TASK_CONFIG     # 任务配置
from config_manager import ConfigManager    # 配置管理器
from logger import get_logger, TaskLogger, LogLevel  # 日志记录工具

class GomerXRobot:
    """
    GomerX 机器人自动化任务控制器
    
    这个类是整个机器人控制系统的核心，负责协调所有硬件模块和软件组件，
    实现从导航到物品识别再到抓取放置的完整自动化任务流程。
    """
    
    def __init__(self, config_file=None, log_level=LogLevel.INFO):
        """
        初始化机器人
        
        Args:
            config_file: 配置文件路径，如果提供则加载该配置文件
            log_level: 日志级别，控制日志输出的详细程度
        """
        # 初始化日志记录器
        # 日志记录器用于记录程序运行过程中的重要事件和错误信息
        self.logger = get_logger(log_level=log_level)
        self.logger.info("初始化GomerX机器人")
        
        # 初始化配置管理器
        # 配置管理器负责加载和管理所有模块的配置参数
        self.config_manager = ConfigManager()
        if config_file:
            # 如果指定了配置文件，加载配置
            self.logger.info(f"加载配置文件: {config_file}")
        
        # 初始化各模块
        try:
            # 初始化摄像头模块，用于获取图像
            self.camera = Camera(self.config_manager.get_camera_config())
            self.logger.info("摄像头模块初始化完成")
            
            # 初始化视觉处理模块，用于处理图像数据
            self.vision = VisionProcessor(self.config_manager.get_vision_config())
            self.logger.info("视觉处理模块初始化完成")
            
            # 初始化底盘模块，用于控制机器人移动
            self.chassis = Chassis(self.config_manager.get_chassis_config())
            self.logger.info("底盘模块初始化完成")
            
            # 初始化导航模块，结合视觉和底盘实现自主导航
            self.navigator = Navigator(self.vision, self.chassis)
            self.logger.info("导航模块初始化完成")
            
            # 初始化物品识别模块，用于识别和定位目标物品
            self.object_detector = ObjectDetector(self.camera, self.vision)
            self.logger.info("物品识别模块初始化完成")
            
            # 初始化机械臂模块，用于控制机械臂运动
            self.arm = Arm(self.config_manager.get_arm_config())
            self.logger.info("机械臂模块初始化完成")
            
            # 初始化夹爪模块，用于控制夹爪开合
            self.gripper = Gripper(self.config_manager.get_gripper_config())
            self.logger.info("夹爪模块初始化完成")
            
            # 初始化LED控制模块，用于控制机器人前方LED灯
            self.led = LEDController(self.config_manager.get_led_config())
            self.logger.info("LED控制模块初始化完成")
            
            # 加载任务配置
            # 任务配置包含目标颜色、区域标识等任务相关参数
            self.task_config = self.config_manager.get_task_config()
            self.target_color = self.task_config['target_color']  # 设置目标颜色
            self.logger.info(f"任务配置加载完成，目标颜色: {self.task_config['target_color']}")
            
            self.logger.info("GomerX机器人初始化完成")
        except Exception as e:
            # 如果初始化过程中出现异常，记录错误并重新抛出异常
            self.logger.critical(f"机器人初始化失败: {str(e)}")
            raise
        
    def initialize(self):
        """
        初始化机器人所有模块
        
        这个方法负责建立与所有硬件设备的连接，并进行必要的初始设置。
        它与__init__方法的区别是，__init__创建对象实例，而initialize建立实际硬件连接。
        
        Returns:
            bool: 如果所有模块初始化成功返回True，否则返回False
        """
        self.logger.info("正在初始化机器人模块...")
        
        # 连接摄像头
        # 摄像头是视觉系统的基础，必须首先初始化
        if not self.camera.open():
            self.logger.error("摄像头初始化失败")
            return False
        self.logger.info("摄像头初始化成功")
        
        # 连接底盘
        # 底盘负责机器人移动，是导航的基础
        if not self.chassis.connect():
            self.logger.error("底盘初始化失败")
            return False
        self.logger.info("底盘初始化成功")
        
        # 连接机械臂
        # 机械臂负责抓取和放置物品
        if not self.arm.connect():
            self.logger.error("机械臂初始化失败")
            return False
        self.logger.info("机械臂初始化成功")
        
        # 连接夹爪
        # 夹爪是机械臂的末端执行器，负责实际抓取物品
        if not self.gripper.connect():
            self.logger.error("夹爪初始化失败")
            return False
        self.logger.info("夹爪初始化成功")
        
        # 连接LED控制器
        # LED控制器负责控制机器人前方LED灯
        if not self.led.connect():
            self.logger.error("LED控制器初始化失败")
            return False
        self.logger.info("LED控制器初始化成功")
        
        # 设置目标颜色
        # 告诉物品识别模块要寻找什么颜色的物品
        self.object_detector.set_target_color(self.target_color)
        
        # 移动机械臂到安全位置
        # 安全位置是机械臂的默认位置，避免碰撞
        self.arm.move_to_safe_position()
        
        # 打开夹爪
        # 确保夹爪处于打开状态，准备抓取物品
        self.gripper.open()
        
        self.is_initialized = True
        self.logger.info("机器人初始化完成")
        return True
    
    def shutdown(self):
        """
        关闭机器人所有模块
        
        这个方法负责安全地关闭所有硬件设备，释放资源。
        它会确保机械臂移动到安全位置，夹爪释放物品，然后断开所有连接。
        """
        self.logger.info("正在关闭机器人模块...")
        
        try:
            # 移动机械臂到安全位置
            # 首先将机械臂移动到安全位置，避免在关闭过程中造成碰撞
            if hasattr(self, 'arm') and self.arm.is_connected:
                self.arm.move_to_safe_position()
                self.logger.info("机械臂已移动到安全位置")
            
            # 打开夹爪释放物品
            # 如果夹爪正在持有物品，先释放它
            if hasattr(self, 'gripper') and self.gripper.is_connected and self.gripper.is_holding_object():
                self.gripper.open()
                self.logger.info("夹爪已释放")
            
            # 关闭所有模块
            # 按照与初始化相反的顺序关闭模块
            
            # 关闭摄像头
            if hasattr(self, 'camera') and self.camera.is_opened:
                self.camera.close()
                self.logger.info("摄像头已关闭")
            
            # 断开底盘连接
            if hasattr(self, 'chassis') and self.chassis.is_connected:
                self.chassis.disconnect()
                self.logger.info("底盘连接已断开")
            
            # 断开机械臂连接
            if hasattr(self, 'arm') and self.arm.is_connected:
                self.arm.disconnect()
                self.logger.info("机械臂连接已断开")
            
            # 断开夹爪连接
            if hasattr(self, 'gripper') and self.gripper.is_connected:
                self.gripper.disconnect()
                self.logger.info("夹爪连接已断开")
            
            # 断开LED控制器连接
            if hasattr(self, 'led') and self.led.is_connected:
                self.led.disconnect()
                self.logger.info("LED控制器连接已断开")
            
            self.is_initialized = False
            self.logger.info("机器人已关闭")
        except Exception as e:
            # 即使关闭过程中出现错误，也要记录并继续关闭其他设备
            self.logger.error(f"关闭硬件时出错: {str(e)}")
    
    def navigate_to_item_area(self):
        """
        导航到物品区域
        
        使用循迹导航功能，控制机器人沿着地面上的引导线移动到物品区域。
        物品区域是预先设定的放置目标物品的区域。
        
        Returns:
            bool: 如果成功导航到物品区域返回True，否则返回False
        """
        self.logger.info("开始导航到物品区域...")
        
        # 使用循迹导航到物品区域
        # follow_line方法会控制机器人沿着地面上的线条移动
        # max_duration参数设置最大导航时间，防止无限等待
        success = self.navigator.follow_line(max_duration=20)
        
        if success:
            self.logger.info("已到达物品区域")
            return True
        else:
            self.logger.error("导航到物品区域失败")
            return False
    
    def find_and_pick_object(self, max_attempts=3):
        """
        查找并抓取目标物品
        
        这个方法实现了完整的物品识别和抓取流程：
        1. 使用摄像头检测目标颜色的物品
        2. 选择最近的物品作为目标
        3. 调整机器人位置，使物品进入抓取范围
        4. 控制机械臂和夹爪完成抓取动作
        
        Args:
            max_attempts: 最大尝试次数，防止无限尝试
            
        Returns:
            bool: 如果成功抓取物品返回True，否则返回False
        """
        self.logger.info(f"开始查找并抓取 {self.target_color} 色物品...")
        
        # 循环尝试抓取，直到成功或达到最大尝试次数
        for attempt in range(1, max_attempts + 1):
            self.logger.info(f"抓取尝试 {attempt}/{max_attempts}")
            
            # 检测目标颜色的物品
            # detect_objects方法会返回所有检测到的目标颜色物品的信息
            objects = self.object_detector.detect_objects(self.target_color)
            
            if not objects:
                self.logger.warning(f"未检测到 {self.target_color} 色物品")
                # 尝试小幅转动寻找物品
                # 如果当前视野中没有目标物品，稍微转动机器人扩大搜索范围
                self.chassis.turn_left(angle=0.2)  # 转动约11.5度
                time.sleep(1)  # 等待转动完成
                continue
            
            # 找到最近的物品
            # 从所有检测到的物品中选择距离最近的作为目标
            target_object = self.object_detector.find_closest_object(self.target_color)
            
            if not target_object:
                self.logger.warning("无法确定目标物品")
                continue
            
            # 检查物品是否在合适的抓取范围内
            # is_object_in_range方法判断物品是否在机械臂可以抓取的范围内
            if not self.object_detector.is_object_in_range(target_object):
                # 调整位置，使物品进入抓取范围
                offset = target_object["offset"]  # 获取物品相对于图像中心的偏移量
                self.logger.info(f"物品不在抓取范围内，偏移量: {offset}")
                
                # 调整机器人位置
                # adjust_position_to_object方法会根据偏移量调整机器人位置
                self.navigator.adjust_position_to_object(offset)
                time.sleep(1)  # 等待位置调整完成
                
                # 重新检测物品
                # 位置调整后，重新检测物品以获取更新后的位置信息
                objects = self.object_detector.detect_objects(self.target_color)
                if objects:
                    target_object = self.object_detector.find_closest_object(self.target_color)
                else:
                    continue
            
            # 计算抓取位置
            # 这里简化处理，假设图像中心对应机械臂正前方
            # 实际应用中需要进行坐标转换，将图像坐标转换为机器人坐标系中的位置
            pick_x = 0  # 机械臂正前方
            pick_y = 100  # 距离机器人100mm
            
            # 打开夹爪
            # 确保夹爪处于打开状态，准备抓取
            self.gripper.open()
            time.sleep(0.5)  # 等待夹爪完全打开
            
            # 抓取物品
            # pick_object方法控制机械臂移动到指定位置并执行抓取动作
            if self.arm.pick_object(pick_x, pick_y):
                # 关闭夹爪
                # 机械臂到达位置后，关闭夹爪夹住物品
                self.gripper.close()
                time.sleep(0.5)  # 等待夹爪完全关闭
                
                # 检查是否成功抓取
                # is_holding_object方法检查夹爪是否成功夹住物品
                if self.gripper.is_holding_object():
                    self.logger.info("成功抓取物品")
                    return True
                else:
                    self.logger.warning("夹爪未检测到物品，抓取失败")
            else:
                self.logger.warning("机械臂抓取失败")
        
        self.logger.error(f"经过 {max_attempts} 次尝试，未能成功抓取物品")
        return False
    
    def navigate_to_drop_area(self):
        """
        导航到放置区域
        
        使用循迹导航功能，控制机器人沿着地面上的引导线移动到放置区域。
        放置区域是预先设定的用于放置抓取物品的区域。
        
        Returns:
            bool: 如果成功导航到放置区域返回True，否则返回False
        """
        self.logger.info("开始导航到放置区域...")
        
        # 使用循迹导航到放置区域
        # follow_line方法会控制机器人沿着地面上的线条移动
        # max_duration参数设置最大导航时间，防止无限等待
        success = self.navigator.follow_line(max_duration=20)
        
        if success:
            self.logger.info("已到达放置区域")
            return True
        else:
            self.logger.error("导航到放置区域失败")
            return False
    
    def place_object(self):
        """
        放置物品
        
        控制机械臂移动到指定位置，然后打开夹爪释放物品。
        这个方法假设机器人已经导航到放置区域，并且夹爪正持有物品。
        
        Returns:
            bool: 如果成功放置物品返回True，否则返回False
        """
        self.logger.info("开始放置物品...")
        
        # 计算放置位置
        # 这里简化处理，假设放置区域在机械臂正前方
        # 实际应用中可能需要通过视觉识别放置区域的具体位置
        place_x = 0  # 机械臂正前方
        place_y = 100  # 距离机器人100mm
        
        # 放置物品
        # place_object方法控制机械臂移动到指定位置并执行放置动作
        if self.arm.place_object(place_x, place_y):
            # 打开夹爪释放物品
            # 机械臂到达位置后，打开夹爪释放物品
            self.gripper.open()
            time.sleep(0.5)  # 等待夹爪完全打开
            
            # 检查是否成功释放
            # is_holding_object方法检查夹爪是否还持有物品
            if not self.gripper.is_holding_object():
                self.logger.info("成功放置物品")
                return True
            else:
                self.logger.warning("夹爪未释放物品，放置失败")
        else:
            self.logger.warning("机械臂放置失败")
        
        return False
    
    def run_automatic_task(self):
        """
        执行完整的自动化任务流程
        
        这个方法实现了从开始到结束的完整自动化任务：
        1. 导航到物品区域
        2. 查找并抓取目标物品
        3. 导航到放置区域
        4. 放置物品
        
        使用TaskLogger记录每个步骤的执行情况和时间。
        
        Returns:
            bool: 如果任务成功完成返回True，否则返回False
        """
        # 使用TaskLogger记录整个任务的执行过程
        # TaskLogger是一个上下文管理器，会自动记录任务开始和结束时间
        with TaskLogger("完整自动化任务", self.logger) as task_logger:
            try:
                # 导航到物品区域
                task_logger.log_step("导航到物品区域")
                if not self.navigate_to_item_area():
                    self.logger.error("导航到物品区域失败，任务终止")
                    return False
                
                # 查找并抓取物品
                task_logger.log_step("查找并抓取物品")
                if not self.find_and_pick_object():
                    self.logger.error("查找并抓取物品失败，任务终止")
                    return False
                
                # 移动机械臂到安全位置
                # 抓取物品后，将机械臂移动到安全位置，避免在移动过程中碰撞
                self.arm.move_to_safe_position()
                
                # 导航到放置区域
                task_logger.log_step("导航到放置区域")
                if not self.navigate_to_drop_area():
                    self.logger.error("导航到放置区域失败，任务终止")
                    return False
                
                # 放置物品
                task_logger.log_step("放置物品")
                if not self.place_object():
                    self.logger.error("放置物品失败，任务终止")
                    return False
                
                # 移动机械臂到安全位置
                # 放置物品后，将机械臂移动到安全位置
                self.arm.move_to_safe_position()
                
                self.logger.info("自动化任务完成")
                return True
            except Exception as e:
                # 捕获任务执行过程中的异常，记录错误信息
                self.logger.error(f"任务执行过程中发生错误: {str(e)}")
                return False
    
    def run_manual_mode(self):
        """
        运行手动模式，用于调试和测试
        
        手动模式允许用户通过命令行输入直接控制机器人的各个组件，
        这对于调试和测试非常有用。用户可以控制机器人移动、操作机械臂和夹爪等。
        """
        self.logger.info("进入手动模式，输入命令控制机器人...")
        print("进入手动模式，输入命令控制机器人...")
        print("可用命令:")
        print("  forward - 前进")
        print("  backward - 后退")
        print("  left - 左转")
        print("  right - 右转")
        print("  stop - 停止")
        print("  open - 打开夹爪")
        print("  close - 关闭夹爪")
        print("  safe - 移动到安全位置")
        print("  detect - 检测物品")
        print("  quit - 退出手动模式")
        
        # 持续等待用户输入，直到输入quit命令
        while True:
            try:
                # 获取用户输入的命令
                command = input("请输入命令: ").strip().lower()
                
                # 处理不同的命令
                if command == "quit":
                    break  # 退出循环，结束手动模式
                elif command == "forward":
                    # 控制底盘前进
                    self.chassis.move_forward()
                elif command == "backward":
                    # 控制底盘后退
                    self.chassis.move_backward()
                elif command == "left":
                    # 控制底盘左转
                    self.chassis.turn_left()
                elif command == "right":
                    # 控制底盘右转
                    self.chassis.turn_right()
                elif command == "stop":
                    # 停止底盘移动
                    self.chassis.stop()
                elif command == "open":
                    # 打开夹爪
                    self.gripper.open()
                elif command == "close":
                    # 关闭夹爪
                    self.gripper.close()
                elif command == "safe":
                    # 移动机械臂到安全位置
                    self.arm.move_to_safe_position()
                elif command == "detect":
                    # 检测物品
                    objects = self.object_detector.detect_objects(self.target_color)
                    if objects:
                        print(f"检测到 {len(objects)} 个 {self.target_color} 色物品")
                    else:
                        print(f"未检测到 {self.target_color} 色物品")
                else:
                    # 未知命令
                    print("未知命令")
                    
            except KeyboardInterrupt:
                # 捕获Ctrl+C中断信号
                break
            except Exception as e:
                # 捕获其他异常
                print(f"命令执行失败: {e}")
        
        print("退出手动模式")
        self.logger.info("退出手动模式")


def main():
    """
    主函数
    
    负责解析命令行参数，初始化日志记录器，创建机器人实例，
    并根据指定的模式执行相应的操作。
    """
    # 解析命令行参数
    # argparse模块提供了强大的命令行参数解析功能
    parser = argparse.ArgumentParser(description="GomerX 机器人自动化任务程序")
    parser.add_argument("--mode", choices=["auto", "manual", "config"], default="auto",
                        help="运行模式: auto(自动), manual(手动), config(配置)")
    parser.add_argument("--config", help="配置文件路径")
    parser.add_argument("--log-level", choices=["debug", "info", "warning", "error"], 
                       default="info", help="日志级别")
    
    # 解析命令行参数
    args = parser.parse_args()
    
    # 转换日志级别
    # 将字符串形式的日志级别转换为LogLevel枚举值
    log_level_map = {
        "debug": LogLevel.DEBUG,
        "info": LogLevel.INFO,
        "warning": LogLevel.WARNING,
        "error": LogLevel.ERROR
    }
    log_level = log_level_map.get(args.log_level, LogLevel.INFO)
    
    # 初始化日志记录器
    logger = get_logger(log_level=log_level)
    logger.info("GomerX机器人控制程序启动")
    
    # 如果是配置模式，直接进入配置菜单
    if args.mode == "config":
        from config_manager import config_menu
        config_menu()
        return
    
    try:
        # 创建机器人实例
        # 根据命令行参数创建机器人实例
        robot = GomerXRobot(config_file=args.config, log_level=log_level)
        
        # 初始化机器人
        # 建立与所有硬件设备的连接
        if not robot.initialize():
            logger.error("机器人初始化失败，程序退出")
            return
        
        # 根据模式执行相应操作
        if args.mode == "auto":
            # 自动模式：执行完整的自动化任务
            logger.info("启动自动模式")
            success = robot.run_automatic_task()
            if success:
                logger.info("任务执行成功")
            else:
                logger.error("任务执行失败")
        elif args.mode == "manual":
            # 手动模式：允许用户通过命令控制机器人
            logger.info("启动手动模式")
            robot.run_manual_mode()
    except KeyboardInterrupt:
        # 捕获Ctrl+C中断信号
        logger.info("用户中断程序")
    except Exception as e:
        # 捕获其他异常
        logger.critical(f"程序运行时发生严重错误: {str(e)}")
    finally:
        # 确保在程序退出前关闭机器人
        if 'robot' in locals():
            robot.shutdown()
        logger.info("程序结束")


# 程序入口点
# 当直接运行此文件时，执行main函数
if __name__ == "__main__":
    main()